/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/* Author: Brian Gerkey */

#define USAGE "\nUSAGE: map_server <map.yaml>\n" \
              "  map.yaml: map description file\n" \
              "DEPRECATED USAGE: map_server <map> <resolution>\n" \
              "  map: image file to load\n"\
              "  resolution: map resolution [meters/pixel]"

#include <stdio.h>
#include <stdlib.h>
#include <fstream>
#include <boost/filesystem.hpp>

#include "ros/ros.h"
#include "ros/console.h"
#include "map_server/image_loader.h"
#include "nav_msgs/MapMetaData.h"
#include "nav_msgs/LoadMap.h"
#include "yaml-cpp/yaml.h"

#ifdef HAVE_YAMLCPP_GT_0_5_0
// The >> operator disappeared in yaml-cpp 0.5, so this function is
// added to provide support for code written under the yaml-cpp 0.3 API.
template<typename T>
void operator >> (const YAML::Node& node, T& i)
{
  i = node.as<T>();
}
#endif


/*******************************************************************************************
ROS中的地图很好理解，就是一张普通的灰度图像，通常为pgm格式。在地图中，黑色的网格是障碍区域，白色的网格是无障碍区域，
灰色的网格是未知区域。map_server从图片中读取信息然后将每个网格都赋值[-1,100]，灰色是-1，代表未知区域；白色是0，空闲区域；黑色是100，障碍区域。

map_server发布/map，需要yaml和pgm两个文件，然后可以通过指令来加载这张地图,map_server相关命令如下:
rosrun map_server map_server    test.yaml 	加载自定义的地图，通常使用roslaunch，如下：
<arg name="map_file" default="/home/hlhp/robot_ws/workspace/maps/test.yaml"/>
<node name="map_server" type="map_server" args="$(arg map_file)" pkg="map_server"/>

https://charon-cheung.github.io/2019/09/15/%E8%B7%AF%E5%BE%84%E8%A7%84%E5%88%92/%E4%BB%A3%E4%BB%B7%E5%9C%B0%E5%9B%BE/ROS%E5%9C%B0%E5%9B%BE%E8%AF%A6%E8%A7%A3/#nav-msgs-OccupancyGrid


map_server加载不同大小地图的影响：
https://charon-cheung.github.io/2021/11/04/%E8%B7%AF%E5%BE%84%E8%A7%84%E5%88%92/%E4%BB%A3%E4%BB%B7%E5%9C%B0%E5%9B%BE/map_server%E5%8A%A0%E8%BD%BD%E4%B8%8D%E5%90%8C%E5%A4%A7%E5%B0%8F%E5%9C%B0%E5%9B%BE%E7%9A%84%E5%BD%B1%E5%93%8D/#%E5%B0%8F%E5%9C%B0%E5%9B%BE
*******************************************************************************************/
class MapServer
{
  public:
    /** Trivial constructor 平凡的构造函数 */
    MapServer(const std::string& fname, double res)
    {
      std::string mapfname = "";
      double origin[3];
      int negate;
      double occ_th, free_th;
      MapMode mode = TRINARY;
      ros::NodeHandle private_nh("~");
      private_nh.param("frame_id", frame_id_, std::string("map"));  //地图坐标系的名称，要在已发布地图的标题中设置的框架， 绑定发布的地图与tf中的哪个frame，通常就是map
      
      get_map_service_ = nh_.advertiseService("static_map", &MapServer::mapCallback, this);      // 请求后返回当前地图的复制
      change_map_srv_ = nh_.advertiseService("change_map", &MapServer::changeMapCallback, this); // 改变当前发布的地图
      // 发布地图的描述信息，以/map_metadata为话题，map_metadata表示地图描述信息如长、宽、分辨率
      metadata_pub_ = nh_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
      // 发布锁存的地图消息，以/map为话题，地图中栅格内容

      //地图在ROS中是以Topic的形式维护和呈现的，这个Topic名称就叫做/map，它的消息类型是nav_msgs/OccupancyGrid。由于/map中实际上存储的是一张图片，
      //为了减少不必要的开销，这个Topic往往采用锁存（latched）的方式来发布。什么是锁存？其实就是：地图如果没有更新，就维持着上次发布的内容不变，此时如果
      //有新的订阅者订阅消息，这时只会收到一个/map的消息，也就是上次发布的消息；只有地图更新了（比如SLAM又建出来新的地图），这时/map才会发布新的内容。
      
      //锁存器的作用就是，将发布者最后一次发布的消息保存下来，然后把它自动发送给后来的订阅者。这种方式非常适合变动较慢、相对固定的数据（例如地图），然后只发布一次，
      //相比于同样的消息不定的发布，锁存的方式既可以减少通信中对带宽的占用，也可以减少消息资源维护的开销。
      // https://charon-cheung.github.io/2019/09/15/%E8%B7%AF%E5%BE%84%E8%A7%84%E5%88%92/%E4%BB%A3%E4%BB%B7%E5%9C%B0%E5%9B%BE/ROS%E5%9C%B0%E5%9B%BE%E8%AF%A6%E8%A7%A3/#%E9%94%81%E5%AD%98
      map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);

      deprecated_ = (res != 0);
      if (!deprecated_) {
        if (!loadMapFromYaml(fname)){
          exit(-1);
        }
      } else {
        if (!loadMapFromParams(fname, res)){
          exit(-1);
        }
      }
    }

  private:
    ros::NodeHandle nh_;
    ros::Publisher map_pub_;
    ros::Publisher metadata_pub_;
    ros::ServiceServer get_map_service_;
    ros::ServiceServer change_map_srv_;
    bool deprecated_;
    std::string frame_id_;

    /** Callback invoked(请求) when someone requests our service 当有人请求我们的服务时调用的回调 */
    bool mapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res ){
      // request is empty; we ignore it

      // = operator is overloaded to make deep copy (tricky!)运算符重载以进行深层复制（棘手！）
      res = map_resp_;
      ROS_INFO("Sending map");

      return true;
    }

    /** Callback invoked when someone requests to change the map */
    bool changeMapCallback(nav_msgs::LoadMap::Request  &request, nav_msgs::LoadMap::Response &response ){
      if (loadMapFromYaml(request.map_url)){
        response.result = response.RESULT_SUCCESS;
        ROS_INFO("Changed map to %s", request.map_url.c_str());
      }
      else{
        response.result = response.RESULT_UNDEFINED_FAILURE;
      }
      return true;
    }

    /** Load a map given all the values needed to understand it
     */
    bool loadMapFromValues(std::string map_file_name, double resolution, int negate, double occ_th, double free_th, double origin[3], MapMode mode){
      ROS_INFO("Loading map from image \"%s\"", map_file_name.c_str());
      try {
        map_server::loadMapFromFile(&map_resp_, map_file_name.c_str(), resolution, negate, occ_th, free_th, origin, mode);
      } catch (std::runtime_error& e) {
        ROS_ERROR("%s", e.what());
        return false;
      }

      // To make sure get a consistent time in simulation  确保在模拟中获得一致的时间
      ros::Time::waitForValid();
      map_resp_.map.info.map_load_time = ros::Time::now();
      map_resp_.map.header.frame_id = frame_id_;
      map_resp_.map.header.stamp = ros::Time::now();
      ROS_INFO("Read a %d X %d map @ %.3lf m/cell", map_resp_.map.info.width, map_resp_.map.info.height, map_resp_.map.info.resolution);
      meta_data_message_ = map_resp_.map.info;

      //Publish latched topics
      metadata_pub_.publish( meta_data_message_ );
      map_pub_.publish( map_resp_.map );
      return true;
    }

    /** Load a map using the deprecated method
     */
    bool loadMapFromParams(std::string map_file_name, double resolution){
      ros::NodeHandle private_nh("~");
      int negate;
      double occ_th;
      double free_th;
      double origin[3];
      private_nh.param("negate", negate, 0);
      private_nh.param("occupied_thresh", occ_th, 0.65);
      private_nh.param("free_thresh", free_th, 0.196);
      origin[0] = origin[1] = origin[2] = 0.0;
      return loadMapFromValues(map_file_name, resolution, negate, occ_th, free_th, origin, TRINARY);
    }

    /** Load a map given a path to a yaml file   加载给定 yaml 文件路径的地图
     */
    bool loadMapFromYaml(std::string path_to_yaml) {
      std::string mapfname;
      MapMode mode;
      double res;
      int negate;
      double occ_th;
      double free_th;
      double origin[3];
      std::ifstream fin(path_to_yaml.c_str());
      if (fin.fail()) {
        ROS_ERROR("Map_server could not open %s.", path_to_yaml.c_str());
        return false;
      }
     #ifdef HAVE_YAMLCPP_GT_0_5_0
      // The document loading process changed in yaml-cpp 0.5.
      YAML::Node doc = YAML::Load(fin);
     #else
      YAML::Parser parser(fin);
      YAML::Node doc;
      parser.GetNextDocument(doc);
     #endif
      try {
        doc["resolution"] >> res;//地图分辨率 单位：米/像素
      } catch (YAML::InvalidScalar &) {
        ROS_ERROR("The map does not contain a resolution tag or it is invalid.");
        return false;
      }
      try {
        doc["negate"] >> negate;//是否应该颠倒 白：自由/黑：的语义(阈值的解释不受影响)
      } catch (YAML::InvalidScalar &) {
        ROS_ERROR("The map does not contain a negate tag or it is invalid.");
        return false;
      }
      try {
        doc["occupied_thresh"] >> occ_th;//占用概率大于此阈值的像素被认为已完全占用。占用概率，如当没有障碍物占据时，白色RGB值为255，则占用概率occ=(255-255)/255=0, 没有被占据。
      } catch (YAML::InvalidScalar &) {
        ROS_ERROR("The map does not contain an occupied_thresh tag or it is invalid.");
        return false;
      }
      try {
        doc["free_thresh"] >> free_th;//占用率小于此阈值的像素被认为是完全空闲的
      } catch (YAML::InvalidScalar &) {
        ROS_ERROR("The map does not contain a free_thresh tag or it is invalid.");
        return false;
      }
      try {
        std::string modeS = "";
        doc["mode"] >> modeS;

        if(modeS=="trinary")
          mode = TRINARY;
        else if(modeS=="scale")
          mode = SCALE;
        else if(modeS=="raw")
          mode = RAW;
        else{
          ROS_ERROR("Invalid mode tag \"%s\".", modeS.c_str());
          return false;
        }
      } catch (YAML::Exception &) {
        ROS_DEBUG("The map does not contain a mode tag or it is invalid... assuming Trinary");
        mode = TRINARY;
      }
      try {
        doc["origin"][0] >> origin[0];// 图像左下角在地图坐标系下的位姿
        doc["origin"][1] >> origin[1];
        doc["origin"][2] >> origin[2];
      } catch (YAML::InvalidScalar &) {
        ROS_ERROR("The map does not contain an origin tag or it is invalid.");
        return false;
      }
      try {
        doc["image"] >> mapfname;
        // TODO: make this path-handling more robust
        if(mapfname.size() == 0){
          ROS_ERROR("The image tag cannot be an empty string.");
          return false;
        }

        // is_absolute获取文件的绝对路径，如果yaml文件中，设置的不是绝对路径,则进行路径拼接
        boost::filesystem::path mapfpath(mapfname);
        if (!mapfpath.is_absolute()){
          boost::filesystem::path dir(path_to_yaml); // yaml文件的路径
          dir = dir.parent_path(); //获取父目录
          mapfpath = dir / mapfpath;
          mapfname = mapfpath.string();
        }
      } catch (YAML::InvalidScalar &) {
        ROS_ERROR("The map does not contain an image tag or it is invalid.");
        return false;
      }
      // 将地图信息发布出去
      return loadMapFromValues(mapfname, res, negate, occ_th, free_th, origin, mode);
    }

    /** The map data is cached here, to be sent out to service callers 地图数据缓存在这里，发送给服务调用者
     */
    nav_msgs::MapMetaData meta_data_message_;
    nav_msgs::GetMap::Response map_resp_;

    /*
    void metadataSubscriptionCallback(const ros::SingleSubscriberPublisher& pub)
    {
      pub.publish( meta_data_message_ );
    }
    */

};


/*******************************************************************************************
// main函数中创建了一个名叫MapServer的ROS节点，
// if 检查输入参数是否正常
// 输入进来的应该有两个参数：
// fname: 文件的名字
// res: 分辨率
// 然后创建MapServer类对象ms，并把文件名fname和分辨率res作为初始化ms的初始条件

// 作用：可以直接指定加载map.yaml文件：rosrun map_server map_server mymap.yaml
//       另一种是指定加载map.pgm和指定分辨率(官方说明：不建议使用)
*******************************************************************************************/
int main(int argc, char **argv)
{
  ros::init(argc, argv, "map_server", ros::init_options::AnonymousName); 
  // step 1 : 检查rosrun map_server的命令是否正确
  ros::NodeHandle nh("~");//使用私有化的命名空间
  if(argc != 3 && argc != 2)
  {
    ROS_ERROR("%s", USAGE);
    exit(-1);
  }
  if (argc != 2) {
    ROS_WARN("Using deprecated map server interface. Please switch to new interface.");
  }
  std::string fname(argv[1]);
  double res = (argc == 2) ? 0.0 : atof(argv[2]);

  // step 2: 创建MapServer类对象ms，运行MapServer构造函数
  try
  {
    MapServer ms(fname, res);
    ros::spin();
  }
  catch(std::runtime_error& e)
  {
    ROS_ERROR("map_server exception: %s", e.what());
    return -1;
  }

  return 0;
}
